function [points_3d,projPointsImg1,projPointsImg2] = my_triangulate(points1,camera1, points2,camera2)
points_3d=zeros(size(points1,1),3);
numMatches=size(points1,1);
x1 = cart2hom(points1);
x2 = cart2hom(points2);
projPointsImg1 = zeros(numMatches, 2);
projPointsImg2 = zeros(numMatches, 2);
for i=1:numMatches
    pt1 = x1(i,:);
    pt2 = x2(i,:);
    crossProductMat1 = [  0   -pt1(3)  pt1(2); pt1(3)   0   -pt1(1); -pt1(2)  pt1(1)   0  ];
    crossProductMat2 = [  0   -pt2(3)  pt2(2); pt2(3)   0   -pt2(1); -pt2(2)  pt2(1)   0  ];    
    equation = [ crossProductMat1*camera1; crossProductMat2*camera2 ];
    [~,~,V] = svd(equation);
    triangPoint = V(:,end)';
    points_3d(i,:)=hom2cart(triangPoint);
    projPointsImg1(i,:) = hom2cart((camera1 * triangPoint')');
    projPointsImg2(i,:) = hom2cart((camera2 * triangPoint')');
end
 
% function point3d = triangulateOnePoint(position, cameraMatric)
% position  = position';
% P = cameraMatric';
% A = position(:, 1) * P(3,:) - P(1:2,:);
% [~,~,V] = svd(A);
% X = V(:, end);
% X = X/X(end);
% point3d = X(1:3)'; 

